/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package br.ufrn.engcomp.mpc.domain;

import SSMPC2Java.SSMPC2Java;
import br.ufrn.engcomp.mpc.util.MATLABUtils;
import br.ufrn.engcomp.mpc.util.MathCommonsUtils;
import com.mathworks.toolbox.javabuilder.MWClassID;
import com.mathworks.toolbox.javabuilder.MWComplexity;
import com.mathworks.toolbox.javabuilder.MWException;
import com.mathworks.toolbox.javabuilder.MWNumericArray;
import java.util.logging.Level;
import java.util.logging.Logger;
import org.apache.commons.math.linear.Array2DRowRealMatrix;
import org.apache.commons.math.linear.ArrayRealVector;
import org.apache.commons.math.linear.RealMatrix;

/**
 *
 * @author Felipe
 */
public class MATLABSSPredictiveController extends AbstractPredictiveController {

    private SSMPC2Java matlabSSMPC;
    
    private MWNumericArray powersAMatrix;
    private MWNumericArray powersABMatrix;
    private MWNumericArray multiColumnPowersABMatrix;
    private MWNumericArray identityCMatrix;
    private MWNumericArray psiMatrix;
    private MWNumericArray gammaMatrix;
    private MWNumericArray thetaMatrix;
    private MWNumericArray qMatrix;
    private MWNumericArray rMatrix;
    private MWNumericArray hMatrix;
    private MWNumericArray apostropheGMatrix;
    private MWNumericArray eConstraintsMatrix;
    private MWNumericArray fConstraintsMatrix;
    private MWNumericArray gConstraintsMatrix;
    
    // TODO: Criar método para fazer dispose dos arrays e objetos do MATLAB.
    
    public MATLABSSPredictiveController(Plant plant) {
        super(plant);
    }
    
    @Override
    public double[] synchComputeU(double[] setPoint, double[] lastX, double[] lastY, double[] lastU, double[] lastStep) {
        double[] u = new double[lastU.length];
        int cB = plant.getBMatrix().getColumnDimension();
        int rC = plant.getCMatrix().getRowDimension();
        
        try {
//            initStaticData();
            
            MWNumericArray setPointVector = generateVector(setPoint);
            MWNumericArray lastXVector = generateVector(lastX);
            MWNumericArray lastYVector = generateVector(lastY);
            MWNumericArray lastUVector = generateVector(lastU);
            
            Object[] result;
            
            if (plant.isConstrained()) {
                PlantWithConstraints plantWithConstraints = (PlantWithConstraints) plant;

                RealMatrix staticEConstraintsMatrixSeed = plantWithConstraints.getStaticEConstraintsMatrixSeed();
                RealMatrix staticFConstraintsMatrixSeed = plantWithConstraints.getStaticFConstraintsMatrixSeed();
                RealMatrix staticGConstraintsMatrixSeed = plantWithConstraints.getStaticGConstraintsMatrixSeed();
                RealMatrix dynamicEConstraintsMatrixSeed = plantWithConstraints.getDynamicEConstraintsMatrixSeed();
                RealMatrix dynamicFConstraintsMatrixSeed = plantWithConstraints.getDynamicFConstraintsMatrixSeed(new ArrayRealVector(lastX));
                RealMatrix dynamicGConstraintsMatrixSeed = plantWithConstraints.getDynamicGConstraintsMatrixSeed(new ArrayRealVector(lastStep), new ArrayRealVector(setPoint));
                RealMatrix allEConstraintsMatrixSeed = MathCommonsUtils.concatenateRow(staticEConstraintsMatrixSeed, dynamicEConstraintsMatrixSeed);
                RealMatrix allFConstraintsMatrixSeed = MathCommonsUtils.concatenateRow(staticFConstraintsMatrixSeed, dynamicFConstraintsMatrixSeed);
                RealMatrix allGConstraintsMatrixSeed = MathCommonsUtils.concatenateRow(staticGConstraintsMatrixSeed, dynamicGConstraintsMatrixSeed);
        
                eConstraintsMatrix = generateConstraints(hu, allEConstraintsMatrixSeed);
                fConstraintsMatrix = generateConstraints(hu, allFConstraintsMatrixSeed);
                gConstraintsMatrix = generateConstraints(hp, allGConstraintsMatrixSeed);
                
                result = matlabSSMPC.constrainedSsmpc2Java(1, hp, hu, cB, rC, qMatrix, setPointVector, lastXVector, lastYVector, lastUVector, psiMatrix, gammaMatrix, hMatrix, apostropheGMatrix, powersAMatrix, powersABMatrix, multiColumnPowersABMatrix, eConstraintsMatrix, fConstraintsMatrix, gConstraintsMatrix);
            } else {
                result = matlabSSMPC.ssmpc2Java(1, hp, hu, cB, rC, setPointVector, lastXVector, lastYVector, lastUVector, psiMatrix, gammaMatrix, hMatrix, apostropheGMatrix);
            }
            
            u = ((MWNumericArray) result[0]).getDoubleData();
        } catch (MWException ex) {
            Logger.getLogger(MATLABSSPredictiveController.class.getName()).log(Level.SEVERE, null, ex);
        }
        
        return u;
    }
    
    // NOVO
    @Override
    public double[] synchComputeU(double[] setPoint, double[] lastX, double[] lastY, double[] lastU, double[] lastStep, double d) {
        double[] u = new double[lastU.length];
        int cB = plant.getBMatrix().getColumnDimension();
        int rC = plant.getCMatrix().getRowDimension();
        
        try {
            MWNumericArray lastXVector = generateVector(lastX);
            
            Object[] result = matlabSSMPC.ssmpc2Java(1, hp, hu, cB, rC, setPoint, lastXVector, lastY, lastU, psiMatrix, gammaMatrix, hMatrix, apostropheGMatrix);
            
            u = ((MWNumericArray) result[0]).getDoubleData();
        } catch (MWException ex) {
            Logger.getLogger(MATLABSSPredictiveController.class.getName()).log(Level.SEVERE, null, ex);
        }
        
        return u;
    }

    @Override
    protected final void initEnviroment() {
        initMATLAB();
    }

    private void initMATLAB() {
        try {
            matlabSSMPC = new SSMPC2Java();
        } catch (MWException ex) {
            Logger.getLogger(MATLABSSPredictiveController.class.getName()).log(Level.SEVERE, null, ex);
        }
    }

    @Override
    protected void initStaticData() {
        computePowersAMatrix();
        computePowersABMatrix();
        computeIdentityCMatrix();
        computeMultiColumnPowersABMatrix();
        computePsiMatrix();
        computeGammaMatrix();
        computeThetaMatrix();
        generateQMatrix();
        generateRMatrix();
        computeHMatrix();
        computeApostropheGMatrix();
    }

    private MWNumericArray generateVector(double[] dataArray) {
        MWNumericArray vector = MWNumericArray.newInstance(new int[] { dataArray.length, 1 }, MWClassID.DOUBLE, MWComplexity.REAL);
        
        for (int i = 0; i < dataArray.length; i++) {
            vector.set(new int[] { i + 1, 1 }, dataArray[i]);
        }
        
        return vector;
    }

    private void computePowersAMatrix() {
        MWNumericArray aMatrix = MATLABUtils.convertMatrixJavaToMATLAB(plant.getAMatrix());
        
        try {
            Object[] result = matlabSSMPC.computeAPowers(1, hp, aMatrix);
            
            powersAMatrix = (MWNumericArray) result[0];
        } catch (MWException ex) {
            Logger.getLogger(MATLABSSPredictiveController.class.getName()).log(Level.SEVERE, null, ex);
        }
    }

    private void computePowersABMatrix() {
        int oA = plant.getAMatrix().getRowDimension();
        MWNumericArray bMatrix = MATLABUtils.convertMatrixJavaToMATLAB(plant.getBMatrix());
        
        try {
            Object[] result = matlabSSMPC.computeABPowers(1, hp, oA, powersAMatrix, bMatrix);
            
            powersABMatrix = (MWNumericArray) result[0];
        } catch (MWException ex) {
            Logger.getLogger(MATLABSSPredictiveController.class.getName()).log(Level.SEVERE, null, ex);
        }
    }

    private void computeMultiColumnPowersABMatrix() {
        int oA = plant.getAMatrix().getRowDimension();
        int cB = plant.getBMatrix().getColumnDimension();
        
        try {
            Object[] result = matlabSSMPC.computeMultiColumnABPowers(1, hp, hu, oA, cB, powersABMatrix);
            
            multiColumnPowersABMatrix = (MWNumericArray) result[0];
        } catch (MWException ex) {
            Logger.getLogger(MATLABSSPredictiveController.class.getName()).log(Level.SEVERE, null, ex);
        }
    }

    private void computeIdentityCMatrix() {
        MWNumericArray cMatrix = MATLABUtils.convertMatrixJavaToMATLAB(plant.getCMatrix());
        
        try {
            Object[] result = matlabSSMPC.computeIdentityC(1, hp, cMatrix);
            
            identityCMatrix = (MWNumericArray) result[0];
        } catch (MWException ex) {
            Logger.getLogger(MATLABSSPredictiveController.class.getName()).log(Level.SEVERE, null, ex);
        }
    }

    private void computePsiMatrix() {
        try {
            Object[] result = matlabSSMPC.computePsi(1, identityCMatrix, powersAMatrix);
            
            psiMatrix = (MWNumericArray) result[0];
        } catch (MWException ex) {
            Logger.getLogger(MATLABSSPredictiveController.class.getName()).log(Level.SEVERE, null, ex);
        }
    }

    private void computeGammaMatrix() {
        try {
            Object[] result = matlabSSMPC.computeGamma(1, identityCMatrix, powersABMatrix);
            
            gammaMatrix = (MWNumericArray) result[0];
        } catch (MWException ex) {
            Logger.getLogger(MATLABSSPredictiveController.class.getName()).log(Level.SEVERE, null, ex);
        }
    }

    private void computeThetaMatrix() {        
        try {
            Object[] result = matlabSSMPC.computeTheta(1, identityCMatrix, multiColumnPowersABMatrix);
            
            thetaMatrix = (MWNumericArray) result[0];
        } catch (MWException ex) {
            Logger.getLogger(MATLABSSPredictiveController.class.getName()).log(Level.SEVERE, null, ex);
        }
    }

    private void generateQMatrix() {
        int rC = plant.getCMatrix().getRowDimension();
        RealMatrix subMatrix = new Array2DRowRealMatrix(rC, rC);
        RealMatrix qJavaMatrix = MathCommonsUtils.eye(rC * hp);
        /*RealMatrix zerosTop = MathCommonsUtils.zeros((hw - 1) * rC, hp * rC);
        RealMatrix zerosLeft = MathCommonsUtils.zeros((hp - hw + 1) * rC, (hw - 1) * rC);
        
        if (zerosTop != null) {
            qMatrix.setSubMatrix(zerosTop.getData(), 0, 0);
        }
        
        if (zerosLeft != null) {
            qMatrix.setSubMatrix(zerosLeft.getData(), (hw - 1) * rC + 1, 0);
        }*/
        
        for (int i = 0; i < rC; i++) {
            subMatrix.setEntry(i, i, q[i]);
        }
        
        int startPointer;
        
        for (int i = (hw - 1); i < hp; i++) {
            startPointer = i * rC;
            
            qJavaMatrix.setSubMatrix(subMatrix.getData(), startPointer, startPointer);
        }
        
        //TODO: qJavaMatrix.scalarMultiply(getQ());
        
        qMatrix = MATLABUtils.convertMatrixJavaToMATLAB(qJavaMatrix);
    }

    private void generateRMatrix() {
        int cB = plant.getBMatrix().getColumnDimension();
        RealMatrix subMatrix = new Array2DRowRealMatrix(cB, cB);
        RealMatrix rJavaMatrix = MathCommonsUtils.eye(cB * hu);
        
        for (int i = 0; i < cB; i++) {
            subMatrix.setEntry(i, i, r[i]);
        }
        
        int startPointer;
        
        for (int i = 0; i < hu; i++) {
            startPointer = i * cB;
            
            rJavaMatrix.setSubMatrix(subMatrix.getData(), startPointer, startPointer);
        }
        
        rMatrix = MATLABUtils.convertMatrixJavaToMATLAB(rJavaMatrix);
    }

    private void computeHMatrix() {
        try {
            Object[] result = matlabSSMPC.computeH(1, thetaMatrix, qMatrix, rMatrix);
            
            hMatrix = (MWNumericArray) result[0];
        } catch (MWException ex) {
            Logger.getLogger(MATLABSSPredictiveController.class.getName()).log(Level.SEVERE, null, ex);
        }
    }

    private void computeApostropheGMatrix() {
        try {
            Object[] result = matlabSSMPC.computeApostropheG(1, thetaMatrix, qMatrix);
            
            apostropheGMatrix = (MWNumericArray) result[0];
        } catch (MWException ex) {
            Logger.getLogger(MATLABSSPredictiveController.class.getName()).log(Level.SEVERE, null, ex);
        }
    }
    
    private MWNumericArray generateConstraints(int horizon, RealMatrix constraintsSeed) {
        try {
            Object[] result = matlabSSMPC.generateConstraints(1, horizon, MATLABUtils.convertMatrixJavaToMATLAB(constraintsSeed));

        return (MWNumericArray) result[0];
        } catch (MWException ex) {
            Logger.getLogger(MATLABSSPredictiveController.class.getName()).log(Level.SEVERE, null, ex);
        }
        
        return MWNumericArray.newInstance(new int[] { 0, 0 }, MWClassID.DOUBLE, MWComplexity.REAL);
    }
}
